#include "PS.h"
#include "stm32f4xx.h"
#include "gpio.h"
#include "commData.h"
#include "motor.h"
#include "GlobalData.h"


uint8_t HAND;
uint8_t keybuf0;
uint8_t keybuf1;
uint8_t RES[6];
uint8_t t = 0;

void PS_InOut(void)
{
    uint8_t buf, duf = 0;
    uint8_t i,j = 1;
    buf = HAND;
    for (i = 0; i <=7 ; ++i)
    {
        GPIO_SetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
        if(buf & 0x01)
            GPIO_SetBits(PS_CMD_Port, PS_CMD_Pin);
        else
            GPIO_ResetBits(PS_CMD_Port, PS_CMD_Pin);
        buf = buf >> 1;
        GPIO_ResetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
        if(GPIO_ReadInputDataBit(PS_DAT_Port, PS_DAT_Pin))
            duf = duf + j;
        j = j * 2;
        GPIO_SetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
    }
    RES[t++] = duf;
}

void PS_Out(void)
{
    uint8_t j, k;
    uint8_t duf = 0;
    j = 1;
    for(k = 0; k <= 7; k++)
    {
        GPIO_SetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
        GPIO_ResetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
        if(GPIO_ReadInputDataBit(PS_DAT_Port, PS_DAT_Pin))
            duf = duf + j;
        j = j * 2;
        GPIO_SetBits(PS_CLK_Port, PS_CLK_Pin);
        wait_for_timeout(1);//5ms
    }
    RES[t++] = duf;
}

void PS_Scan(void)
{
    t = 0;
    GPIO_ResetBits(PS_ATT_Port, PS_ATT_Pin);
    HAND = 0x01;
    PS_InOut();
    wait_for_timeout(1);//5ms
    HAND = 0x42;
    PS_InOut();
    wait_for_timeout(5);//25ms
    PS_Out();
    wait_for_timeout(5);//25ms
    PS_Out();  //返回第一个字节
    wait_for_timeout(5);//25ms
    PS_Out();  //返回第二个字节
    keybuf0 = RES[3];
    keybuf1 = RES[4];
    wait_for_timeout(2);//25ms
    GPIO_SetBits(PS_ATT_Port, PS_ATT_Pin);
}

// 初始化PS动作标志位
PSFlagManager PS_Flag = {0,0,0,0,0,0,0,0};

void PS_Control(void)
{
    switch(keybuf0)
    {
        case PS_UP:
            leftMotorSpeed.dir = 1;
            rightMotorSpeed.dir = 1;
            MotorSpeedup(2000, 100);
        break;
        case PS_DOWN:
            leftMotorSpeed.dir = 0;
            rightMotorSpeed.dir = 0;
            MotorSpeedup(2000, 100);
        break;
        case PS_RIGHT:
            leftMotorSpeed.dir = 1;
            rightMotorSpeed.dir = 0;
            MotorSpeedup(2000, 100);
        break;
        case PS_LEFT:
            leftMotorSpeed.dir = 0;
            rightMotorSpeed.dir = 1;
            MotorSpeedup(2000, 100);
        break;
        default:
            leftMotorSpeed.dir = 1;
            rightMotorSpeed.dir = 1;
            MotorBreak();
            MotorDriverTask();
        break;
    }
}

